//
// Created by codex on 2024/7/19.
//
#include <stdio.h>
#include "servo_pid.h"
#include "stdlib.h"
#include "PWM.h"

#define pai 3.1415926f
extern char camera_x,camera_y;
servo_pid_obj servo_pid_1;

void servo_pid_start()
{
    servo_pid_1.sta=1;
    servo_pid_1.x=camera_x;
    servo_pid_1.y=camera_y;
}
void servo_pid_myinit(servo_pid_obj *servo_pid)
{
    servo_pid->x=0;
    servo_pid->y=0;
    servo_pid->x_angle=90;
    servo_pid->y_angle=90;
    servo_pid->Kp_x=-0.5f;//x轴反向才正确
    servo_pid->Kp_y=0.5f;
    servo_pid->sta=0;
    servo_pid->end=0;
    servo_pid->nosee_xcalc=1;
//    angle_set(1,120);
//    angle_set(2,90);
}
void limit(servo_pid_obj *servo_pid)
{
    if(servo_pid->y_angle>=120)servo_pid->y_angle=120;
    if(servo_pid->y_angle<=60)servo_pid->y_angle=60;
    if(servo_pid->x_angle>=120)servo_pid->x_angle=120;
    if(servo_pid->x_angle<=60)servo_pid->x_angle=60;
}
 //假设的控制应用函数
void servo_apply_control(float control_x, float control_y,servo_pid_obj *servo_pid)
{
    // 这里应该包含将控制输出应用到系统上的代码
    // 例如，control_x 和 control_y 可以是电机的PWM值
    servo_pid->x_angle=(int)(servo_pid->x_angle+control_x/(pai*5));
    servo_pid->y_angle=(int)(servo_pid->y_angle+control_y/(pai*5));
    limit(servo_pid);
    angle_set(1,servo_pid->x_angle);
    angle_set(2,servo_pid->y_angle);

    //printf("Control Output: x = %f, y = %f\n", control_x, control_y);
}
void servo_pid_update(servo_pid_obj *servo_pid)
{
    static int i=0;
    const int max_iterations = 1000; // 设置迭代次数，避免无限循环

    //若不在视野内则开始平扫
//    if(servo_pid->sta==1&&servo_pid->end==0&&camera_y==0xFF&&camera_x==0xFF)
//    {
//        if(servo_pid_1.y_angle+5<=120&&servo_pid_1.y_angle-5>=60)
//        {
//            servo_pid_1.y_angle=servo_pid_1.y_angle+5*servo_pid_1.nosee_xcalc;
//            angle_set(2,servo_pid_1.y_angle);
//
//        }
//        else
//        {
//            servo_pid_1.nosee_xcalc=-servo_pid_1.nosee_xcalc;
//            servo_pid_1.y_angle=servo_pid_1.y_angle+5*servo_pid_1.nosee_xcalc;
//            angle_set(2,servo_pid_1.y_angle);
//        }
//    }

    //找到开始kp控制
    if(servo_pid->sta==1&&servo_pid->end==0&&camera_y!=0xFF&&camera_x!=0xFF)
    if(i < max_iterations)
    {
        i++;
        // 计算控制输出
        float control_x = -servo_pid->Kp_x * servo_pid->x;
        float control_y = -servo_pid->Kp_y * servo_pid->y;

        // 应用控制输出到系统
        servo_apply_control(control_x, control_y,servo_pid);

        // 假设系统响应控制并更新x和y的值
        // 这里我们使用摄像头的值
        servo_pid->x=camera_y;
        servo_pid->y=camera_x;

        // 检查是否达到目标
        if (abs(servo_pid->x)< 10 && abs(servo_pid->y)< 10)
        { // 定义一个小的阈值来认为达到了目标
            servo_pid->end=0;//1
        }
        else
        {
            servo_pid->end=0;
            i=0;
        }
    }
}
